#include <stdio.h>

#include "memory.h"
#include "g72x.h"
#include "g726.h"


void init_state(uint8_t* state_ptr) {
  g726_init_state((g726_state*)state_ptr);
}

//----------------------------------------
// 16kbps,rate=0
// 24kbps,rate=1
// 32kbps,rate=2
// 40kbps,rate=3
void g726_encode(uint8_t* state,
                 int rate,
                 int16_t *pcm,
                 int input_len,
                 int8_t *bitstream)
{
  printf("size %d", sizeof(g726_state));

  g726_state* state_ptr = (g726_state*)state;

  if (rate==0) {
    // 16kbps, one sample encode to 2bits, so 4 shorts to one encoded byte
    for (int i = 0; i < input_len/4; i++) {
      *(bitstream + i) =
        (((char)(g726_16_encoder(pcm[i*4    ], AUDIO_ENCODING_LINEAR, state_ptr))) << 6) |
        (((char)(g726_16_encoder(pcm[i*4 + 1], AUDIO_ENCODING_LINEAR, state_ptr))) << 4) |
        (((char)(g726_16_encoder(pcm[i*4 + 2], AUDIO_ENCODING_LINEAR, state_ptr))) << 2) |
        (((char)(g726_16_encoder(pcm[i*4 + 3], AUDIO_ENCODING_LINEAR, state_ptr))));
    }
  }else if (rate==1) {
    // 24kbps, not support for now
  }else if (rate ==2) {

    // 32kbps, most commonly used,
    for (int i = 0; i < input_len/2; i++) {
      *(bitstream + i) =
          (((char)(g726_32_encoder(pcm[i * 2], AUDIO_ENCODING_LINEAR, state_ptr))) << 4) |
          (((char)(g726_32_encoder(pcm[i * 2 + 1], AUDIO_ENCODING_LINEAR, state_ptr))));
    }

  }else if (rate==3) {
    // 40kbps,not support for now
  }
}

//-----------------------------------------
void g726_decode(uint8_t* state,
                 int rate,
                 int8_t *bitstream,
                 int inputlen,
                 int16_t *pcm)
{
  g726_state *state_ptr = (g726_state*)state;

  if (rate==0) {

    for (int i = 0; i < inputlen; i++) {
      int in = (int)(((*(bitstream + i)) & (char)192) >> 6);
      pcm[ i*4 ] = g726_16_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);

      in = (int)(((*(bitstream + i)) & (char)48) >> 4);
      pcm[ i*4 + 1 ] = g726_16_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);

      in = (int)(((*(bitstream + i)) & (char)12) >> 2);
      pcm[ i*4 + 2 ] = g726_16_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);

      in = (int)(((*(bitstream + i)) & (char)3));
      pcm[ i*4 + 3 ] = g726_16_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);
    }

  }else if (rate==1) {

  }else if (rate==2) {

    for (int i = 0; i < inputlen; i++) {
      int in = (int)(((*(bitstream + i)) & (char)240) >> 4);
      pcm[i * 2] = g726_32_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);

      in = (int)(((*(bitstream + i)) & (char)15));
      pcm[i * 2 + 1] = g726_32_decoder(in, AUDIO_ENCODING_LINEAR, state_ptr);
    }

  }else if (rate==3) {

  }

}
